// 1.包含头文件
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/int32.hpp"
#include "std_msgs/msg/float64.hpp"
#include <iostream>
#include "canconfig.h"

using namespace std::chrono_literals;

int joint1gp;
int joint2gp;
int joint3gp;

int flag_joint1 = 0;
int flag_joint2 = 0;
int flag_joint3 = 0;

// float joint2p;
// PID控制参数
float Kp1_1; // 上升 // 比例系数
float Kp1_2; // 下降
float Kp2_1; // 上升 // 比例系数
float Kp2_2; // 下降
float Kp3_1; // 上升 // 比例系数
float Kp3_2; // 下降

// 3.自定义节点类；
class Arm_Control : public rclcpp::Node
{
public:
  Arm_Control() : Node("arm_control_node_cpp")
  {
    // 实例化回调组
    callback_group_0 = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
    callback_group_1 = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
    callback_group_2 = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
    callback_group_3 = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
    callback_group_4 = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
    callback_group_5 = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
    callback_group_6 = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
    callback_group_7 = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);

    // 创建订阅选项，并设置回调组
    rclcpp::SubscriptionOptions options_0;
    options_0.callback_group = callback_group_0;
    rclcpp::SubscriptionOptions options_1;
    options_1.callback_group = callback_group_1;
    rclcpp::SubscriptionOptions options_2;
    options_2.callback_group = callback_group_2;
    rclcpp::SubscriptionOptions options_3;
    options_3.callback_group = callback_group_3;
    rclcpp::SubscriptionOptions options_4;
    options_4.callback_group = callback_group_4;
    rclcpp::SubscriptionOptions options_5;
    options_5.callback_group = callback_group_5;
    rclcpp::SubscriptionOptions options_6;
    options_6.callback_group = callback_group_6;
    rclcpp::SubscriptionOptions options_7;
    options_7.callback_group = callback_group_7;

    // 创建订阅方
    this->action_sub_ = this->create_subscription<std_msgs::msg::Int32>("arm_action", 5, std::bind(&Arm_Control::actionCallback, this, std::placeholders::_1), options_1);
    // 订阅来自编码器/ADC的消息
    this->joint1p_sub_ = this->create_subscription<std_msgs::msg::Int32>("joint1_p_", 5, std::bind(&Arm_Control::control_joint1cb, this, std::placeholders::_1), options_4);
    this->joint2p_sub_ = this->create_subscription<std_msgs::msg::Float64>("joint2_p", 5, std::bind(&Arm_Control::control_joint2cb, this, std::placeholders::_1), options_2);
    this->joint3p_sub_ = this->create_subscription<std_msgs::msg::Float64>("joint3_p_", 5, std::bind(&Arm_Control::control_joint3cb, this, std::placeholders::_1), options_6);
    // 订阅来自qt的消息 或 arm_constyle
    this->joint1gp_sub_ = this->create_subscription<std_msgs::msg::Int32>("joint1_gp_", 5, std::bind(&Arm_Control::joint1gb_cb, this, std::placeholders::_1), options_5);
    this->joint2gp_sub_ = this->create_subscription<std_msgs::msg::Int32>("joint2_gp", 5, std::bind(&Arm_Control::joint2gb_cb, this, std::placeholders::_1), options_3);
    this->joint3gp_sub_ = this->create_subscription<std_msgs::msg::Int32>("joint3_gp_", 5, std::bind(&Arm_Control::joint3gb_cb, this, std::placeholders::_1), options_7);

    sendtimer_ = this->create_wall_timer(0.02s, std::bind(&Arm_Control::sendresult, this), callback_group_0);
  }

private:
  // 动作类别
  rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr action_sub_;
  // 发送结果
  rclcpp::TimerBase::SharedPtr sendtimer_;
  // 来自编码器/ADC的消息
  rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr joint1p_sub_;
  rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr joint2p_sub_;
  rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr joint3p_sub_;

  // 来自qt的消息
  rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr joint1gp_sub_;
  rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr joint2gp_sub_;
  rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr joint3gp_sub_;

  // 声明回调组
  rclcpp::CallbackGroup::SharedPtr callback_group_0;
  rclcpp::CallbackGroup::SharedPtr callback_group_1;
  rclcpp::CallbackGroup::SharedPtr callback_group_2;
  rclcpp::CallbackGroup::SharedPtr callback_group_3;
  rclcpp::CallbackGroup::SharedPtr callback_group_4;
  rclcpp::CallbackGroup::SharedPtr callback_group_5;
  rclcpp::CallbackGroup::SharedPtr callback_group_6;
  rclcpp::CallbackGroup::SharedPtr callback_group_7;

  // ---------回调函数---------
  void control_joint1cb(const std_msgs::msg::Int32::SharedPtr joint1p_msg)
  {
    RCLCPP_INFO(this->get_logger(), "JOINT1 CINTROL");
    int joint1p = joint1p_msg->data;
    RCLCPP_INFO(this->get_logger(), "编码器1中获取的JOINT1位置消息joint1p_msg:%d", joint1p);
    usleep(100000);
    if (flag_joint1 == 1)
    {

      if (joint1gp - joint1p > 3)
      {
        // 向编码器数值增大的方向
        SendData(send_joint_v, joint_id, joint1_f, 4); // 正转
        joint_move1[0] = joint1_f[0];
      }
      else if (joint1gp - joint1p < - 3)
      {
        // 向编码器数值增减少的方向
        SendData(send_joint_v, joint_id, joint2_r, 4); // 反转
        joint_move1[0] = joint1_r[0];
      }
      else
      {
        // 停止
        RCLCPP_INFO(this->get_logger(), "停止");
        joint_move1[0] = joint_stop[0];
        SendData(send_joint_v, joint_id, joint_stop, 4);
        usleep(100000);
        flag_joint1 == 0;
      }
    }
  }
  // ADC中获取的JOINT2位置消息 由于随着ADC信号更新，所以本身就是循环，不用添加其他循环
  void control_joint2cb(const std_msgs::msg::Float64::SharedPtr joint2p_msg)
  {
    RCLCPP_INFO(this->get_logger(), "JOINT2 CINTROL");
    Kp2_1 = 1;
    Kp2_2 = 1.8;
    float joint2p = joint2p_msg->data;
    RCLCPP_INFO(this->get_logger(), "ADC中获取的JOINT2位置消息joint2p_msg:%f", joint2p);
    usleep(100000);
    if (flag_joint2 == 1)
    {
      float error = joint2gp - joint2p; // 计算位置误差
      float output_1;                   // 控制上升输出
      float output_2;                   // 控制下降输出

      output_1 = Kp2_1 * error; // 根据误差计算控制输出
      output_2 = Kp2_2 * error;

      // 限制输出范围，速度范围为[-100, 100]
      // if (output_1 > 200)
      //   output_1 = 200;
      // else if (output_1 < -200)
      //   output_1 = -200;
      // if (output_2 > 200)
      //   output_2 = 200;
      // else if (output_2 < -200)
      //   output_2 = -200;

      // 根据实际位置控制机械臂动作
      // 允许的位移误差
      if (joint2gp - joint2p > 4)
      {
        // 上升
        RCLCPP_INFO(this->get_logger(), "上升");
        joint2_f[1] = static_cast<BYTE>(0x80 + std::min(127.0f, output_1)); //这一句是为了单独调试时使用的，与 SendData(send_joint_v, joint_id, joint2_f, 4); 搭配
        joint_move2[1] = static_cast<BYTE>(0x80 + std::min(127.0f, output_1));
        // SendData(send_joint_v, joint_id, joint2_f, 4);
      }
      else if (joint2gp - joint2p < -4)
      {
        // 下降
        RCLCPP_INFO(this->get_logger(), "下降");
        joint2_r[1] = static_cast<BYTE>(0x80 - std::min(127.0f, -output_2));
        joint_move2[1] = static_cast<BYTE>(0x80 - std::min(127.0f, -output_2));
        // SendData(send_joint_v, joint_id, joint2_r, 4);
      }
      else
      {

        // 停止
        RCLCPP_INFO(this->get_logger(), "停止");
        joint_move2[1] = joint_stop[1];
        // SendData(send_joint_v, joint_id, joint_stop, 4);
        usleep(100000);
        flag_joint2 == 0;
      }
    }
  }

  void control_joint3cb(const std_msgs::msg::Float64::SharedPtr joint3p_msg)
  {
    RCLCPP_INFO(this->get_logger(), "JOINT3 CINTROL");
    Kp3_1 = 1;
    Kp3_2 = 1;
    float joint3p = joint3p_msg->data;
    RCLCPP_INFO(this->get_logger(), "ADC中获取的JOINT2位置消息joint2p_msg:%f", joint3p);
    usleep(100000);
    if (flag_joint3 == 1)
    {
      float error3 = joint3gp - joint3p; // 计算位置误差
      float output3_1;                   // 控制上升输出
      float output3_2;                   // 控制下降输出

      output3_1 = Kp3_1 * error3; // 根据误差计算控制输出
      output3_2 = Kp3_2 * error3;

      // 限制输出范围，速度范围为[-100, 100]
      // if (output3_1 > 200)
      //   output3_1 = 200;
      // else if (output3_1 < -100)
      //   output3_1 = -100;
      // if (output3_2 > 100)
      //   output3_2 = 100;
      // else if (output3_2 < -100)
      //   output3_2 = -100;

      // 根据实际位置控制机械臂动作
      // 允许的位移误差
      if (joint3gp - joint3p > 4)
      {
        // 上升
        RCLCPP_INFO(this->get_logger(), "上升");
        joint3_f[2] = static_cast<BYTE>(0x80 + std::min(127.0f, output3_1));
        joint_move3[2] = static_cast<BYTE>(0x80 + std::min(127.0f, output3_1));
        // SendData(send_joint_v, joint_id, joint3_f, 4);
      }
      else if (joint3gp - joint3p < -4)
      {
        // 下降
        RCLCPP_INFO(this->get_logger(), "下降");
        joint3_r[2] = static_cast<BYTE>(0x80 - std::min(127.0f, -output3_2));
        joint_move3[2] = static_cast<BYTE>(0x80 - std::min(127.0f, -output3_2));
        // SendData(send_joint_v, joint_id, joint3_r, 4);
      }
      else
      {
        // 停止
        RCLCPP_INFO(this->get_logger(), "停止");
        joint_move3[2] = joint_stop[2];
        // SendData(send_joint_v, joint_id, joint_stop, 4);
        usleep(100000);
        flag_joint3 == 0;
      }
    }
  }

  // QT中下发的JOINT位置消息
  void joint1gb_cb(const std_msgs::msg::Int32::SharedPtr joint1gp_msg)
  {
    joint1gp = joint1gp_msg->data;
    flag_joint1 = 1;
    RCLCPP_INFO(this->get_logger(), "joint1gp:%d", joint1gp);
  }
  void joint2gb_cb(const std_msgs::msg::Int32::SharedPtr joint2gp_msg)
  {
    joint2gp = joint2gp_msg->data;
    flag_joint2 = 1;
    // RCLCPP_INFO(this->get_logger(), "joint2gp:%d",joint2gp);
  }
  void joint3gb_cb(const std_msgs::msg::Int32::SharedPtr joint3gp_msg)
  {
    joint3gp = joint3gp_msg->data;
    flag_joint3 = 1;
    // RCLCPP_INFO(this->get_logger(), "joint2gp:%d",joint2gp);
  }

  // 统一发送最终结果
  void sendresult()
  {
    // 一号关节
    joint_move[0] = joint_move1[0];
    // 二号关节
    joint_move[1] = joint_move2[1];
    // 三号关节
    joint_move[2] = joint_move3[2];

    int w;
    printf("\n 综合消息为：");
    for (w = 0; w < 4; w++)
    {
      printf("0x%02X ", joint_move[w]);
    }
    printf("\n");

    // SendData(send_joint_v, joint_id, joint_move, 4);
  }

  // 开环控制
  void actionCallback(const std_msgs::msg::Int32::SharedPtr action_msg)
  {
    RCLCPP_INFO(this->get_logger(), "JOINT-----CINTROL");

    action_flag = action_msg->data;
    switch (action_flag)
    {
    case 0:
      RCLCPP_INFO(this->get_logger(), "全部关节停止");
      SendData(send_joint_v, joint_id, joint_stop, 4);
      break;
    case 1:
      RCLCPP_INFO(this->get_logger(), "一号关节正转");
      SendData(send_joint_v, joint_id, joint1_f, 4);
      break;
    case 2:
      RCLCPP_INFO(this->get_logger(), "一号关节反转");
      SendData(send_joint_v, joint_id, joint1_r, 4);
      break;
    case 3:
      RCLCPP_INFO(this->get_logger(), "二号关节正转");
      SendData(send_joint_v, joint_id, joint2_f, 4);
      break;
    case 4:
      RCLCPP_INFO(this->get_logger(), "二号关节反转");
      SendData(send_joint_v, joint_id, joint2_r, 4);
      break;
    case 5:
      RCLCPP_INFO(this->get_logger(), "三号关节正转");
      SendData(send_joint_v, joint_id, joint3_f, 4);
      break;
    case 6:
      RCLCPP_INFO(this->get_logger(), "三号关节反转");
      SendData(send_joint_v, joint_id, joint3_r, 4);
      break;
    case 7:
      RCLCPP_INFO(this->get_logger(), "四号关节正转");
      SendData(send_joint_v, joint_id, joint4_f, 4);
      break;
    case 8:
      RCLCPP_INFO(this->get_logger(), "四号关节反转");
      SendData(send_joint_v, joint_id, joint4_r, 4);
      break;
    case 9:
      RCLCPP_INFO(this->get_logger(), "Action 9: Perform action 9");
      break;
    case 10:
      RCLCPP_INFO(this->get_logger(), "Action 10: Perform action 10");
      break;
    default:
      RCLCPP_WARN(this->get_logger(), "Unknown action flag: %d", action_flag);
      break;
    }
  }
};

int main(int argc, char const *argv[])
{
  // 2.初始化ROS2客户端；
  rclcpp::init(argc, argv);
  rclcpp::executors::MultiThreadedExecutor executor(rclcpp::ExecutorOptions(), 20, true);
  // 初始化CAN设备
  Init_Can();
  auto node = std::make_shared<Arm_Control>();
  // 4.调用spain函数，并传入节点对象指针；
  executor.add_node(node);
  executor.spin();
  // 5.资源释放
  rclcpp::shutdown();
  // 关闭CAN设备
  VCI_CloseDevice(device_type, device_index);
  return 0;
}

// CAN设备初始化函数
void Init_Can(void)
{
  // 初始化CAN设备
  device_type = VCI_USBCAN2; // 设备类型修改
  device_index = 0;          // 设备的索引
  can_index = 0;             // CAN控制器的索引 CAN1为0，CAN2为1

  if (VCI_OpenDevice(device_type, device_index, 0) != STATUS_OK)
  {
    std::cout << "无法开启CAN设备" << std::endl;
    return;
  }
  else
  {
    std::cout << "设备开启,如需退出请按“CTRL + C”" << std::endl;
  }

  if (VCI_ReadBoardInfo(device_type, device_index, &board_info) != STATUS_OK)
  {
    std::cout << "获取设备信息失败" << std::endl;
    VCI_CloseDevice(device_type, device_index);
    return;
  }
  // 设置CAN初始化配置
  init_config.AccCode = 0X00000000;
  init_config.AccMask = 0xFFFFFFFF;
  init_config.Filter = 2; // 只接受标准帧
  init_config.Mode = 0;
  init_config.Timing0 = 0x01;
  init_config.Timing1 = 0x1c; // 波特率：250kps

  if (VCI_InitCAN(device_type, device_index, can_index, &init_config) != STATUS_OK)
  {
    std::cout << "初始化CAN参数失败" << std::endl;
    VCI_CloseDevice(device_type, device_index);
    return;
  }

  // 开始CAN通讯
  if (VCI_StartCAN(device_type, device_index, can_index) != STATUS_OK)
  {
    std::cout << "开启CAN通道失败" << std::endl;
    VCI_CloseDevice(device_type, device_index);
    return;
  }
}

// 指令发送函数
void SendData(VCI_CAN_OBJ &handle_obj, const int id, const BYTE *data, int dataLen)
{
  handle_obj.ID = id;
  handle_obj.SendType = 1; // 单次发送
  handle_obj.RemoteFlag = 0;
  handle_obj.ExternFlag = 1; // 拓展帧
  handle_obj.DataLen = dataLen;
  for (int i = 0; i < dataLen; i++)
  {
    handle_obj.Data[i] = data[i];
  }
  if (VCI_Transmit(device_type, device_index, can_index, &handle_obj, 1) > 0)
  {
  }
  else
  {
    std::cout << "初始化错误！" << std::endl;
  }
}